/* Blink Example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "sdkconfig.h"

uint8_t step_motor1_pin[4] = {19, 18,  5,  4};
uint8_t step_motor2_pin[4] = {25, 26, 32, 33};
uint8_t step_motor3_pin[4] = {23, 22,  0,  2};

void step_control(uint8_t *pIO, uint8_t step)
{
  gpio_set_level(pIO[0], (step == 0) || (step == 1) || (step == 7));
  gpio_set_level(pIO[1], (step == 1) || (step == 2) || (step == 3));
  gpio_set_level(pIO[2], (step == 3) || (step == 4) || (step == 5));
  gpio_set_level(pIO[3], (step == 5) || (step == 6) || (step == 7));
}

void blink_task(void *pvParameter)
{
    int step1 = 0, step2 = 0, step3 = 0;
    int dir1 = 1, dir2 = 1, dir3 = 1;
    int count1 = 0, count2 = 0, count3 = 0;
    /* Configure the IOMUX register for pad BLINK_GPIO (some pads are
       muxed to GPIO on reset already, but some default to other
       functions and need to be switched to GPIO. Consult the
       Technical Reference for a list of pads and their default
       functions.)
    */
    for(int i = 0; i < 4; i ++) {
        gpio_pad_select_gpio(step_motor1_pin[i]);
        gpio_pad_select_gpio(step_motor2_pin[i]);
        gpio_pad_select_gpio(step_motor3_pin[i]);
        /* Set the GPIO as a push/pull output */
        gpio_set_direction(step_motor1_pin[i], GPIO_MODE_OUTPUT);
        gpio_set_direction(step_motor2_pin[i], GPIO_MODE_OUTPUT);
        gpio_set_direction(step_motor3_pin[i], GPIO_MODE_OUTPUT);
    }
    while(1) {
        step_control(step_motor1_pin, step1);
        step_control(step_motor2_pin, step2);
        step_control(step_motor3_pin, step3);
        step1 += dir1; step2 += dir2; step3 += dir3;
        step1 &= 0x7; step2 &= 0x7; step3 &= 0x7;
        vTaskDelay(1);
        count1 ++; count2 ++; count3 ++;
        if(count1 >= 1000) {
            count1 = 0;
            if(dir1 > 0) dir1 = -1;
            else dir1 = 1;
        }
        if(count2 >= 400) {
            count2 = 0;
            if(dir2 > 0) dir2 = -1;
            else dir2 = 1;
        }
        if(count3 >= 400) {
            count3 = 0;
            if(dir3 > 0) dir3 = -1;
            else dir3 = 1;
        }
    }
}

void app_main()
{
    xTaskCreate(&blink_task, "blink_task", configMINIMAL_STACK_SIZE, NULL, 5, NULL);
}
